/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class CompetitionMain extends IterativeRobot {

	Gyro gyro;
	Joystick leftStick, rightStick, joyPad;
	Encoder leftCoder, rightCoder;
	Solenoid kicker[][], selfRight[][];
	static final int pnFRONT = 2, pnRIGHT = 0, pnBACK = 3, pnLEFT = 1,
		   pnKICK_A = 0, pnKICK_B = 1;
	static final double selfRightDeployTime = .2;
	byte jpU = 4, jpD = 2, jpL = 1, jpR = 3, jpKICK = 5, jpROLL = 6,
		   jsSTRAIGHT = 2, jsTRACK = 4, jsPOINT = 3;
	DigitalInput pressureSwitch;
	byte compressorPin = 1,
		   pressurePin = 14,
		   rollDirection = 1,
		   reloadSettings = 8;
	boolean compressorIsOn = false,
		   kickerCanChangeState = true,
		   kickerIsOut = false,
		   jpKICKhasChangedState = false,
		   enableSelfRighting,
		   enableUltrasonics,
		   isDrivingStraight = false,
		   pneumaticsEnable,
		   visionEnable;
	byte switchNum = 0,
		   numBalls = 1,
		   ballIndex = 0,
		   ballNum = 0;
	double robotX = 93.0,
		   robotY = -159.0,
		   robotAngle = 0.0;
	static double startX[] = {93.0, 57.0, 21.0};
	static double ballX[] = {93.0, 93.0, 93.0};
	static double ballY[] = {-144.0, -108.0, -72.0};
	static double tgtX[] = {0.0, 0.0, 0.0};
	static double tgtY[] = {0.0, 0.0, 0.0};
	static boolean rPositionSwitch[] = {false, true, true};
	static boolean bPositionSwitch[] = {true, true, true, false, true, true, true, true, true};
	static byte psocLEDs = 0x01;
	Relay compressor;
	DigitalModule dm;
	AnalogModule am;
	DriverStationEnhancedIO ds;
	DriverStationLCD dslcd;
	Jaguar leftJag, rightJag;
	Victor rollerJag;
	AxisCamera cam1;
	//Ultrasonic US1, US2;
	CustomUltrasonics ultrasonics;
	EncoderDifference encDiff;
	PIDController lineDrive, servoPID;
	PIDTurn turnPID;
	Servo camServo;
	long periodicCounter = 0;
	//final byte STATE_INIT = 0, STATE_DRIVE = 1, STATE_TURN = 2, STATE_KICK = 3;
	final byte STATE_INIT = 0, STATE_PIVOT_FIRST_BALL = 1, STATE_DRIVE_FIRST_BALL = 2;
	final byte STATE_PIVOT_FIRST_TARGET = 3, STATE_FIRST_KICK = 4, STATE_POST_FIRST_KICK = 5;
	final byte STATE_PIVOT_SECOND_BALL = 6, STATE_DRIVE_SECOND_BALL = 7;
	final byte STATE_PIVOT_SECOND_TARGET = 8, STATE_SECOND_KICK = 9, STATE_POST_SECOND_KICK = 10;
	final byte STATE_PIVOT_THIRD_BALL = 11, STATE_DRIVE_THIRD_BALL = 12;
	final byte STATE_PIVOT_THIRD_TARGET = 13, STATE_THIRD_KICK = 14, STATE_POST_THIRD_KICK = 15;
	final byte STATE_PIVOT_LINE = 16, STATE_DRIVE_LINE = 17, STATE_JUST_LOOK_MENACING = 18;
	final byte STATE_DONE = -1;
	byte autonomousState = STATE_INIT;
	PositionAccumulator position;

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	public void robotInit() {
		getWatchdog().setExpiration(40);
		refreshSettings();

		//getWatchdog().setEnabled(Settings.getBoolean("enableWatchdog"));

		visionEnable = Settings.getBoolean("enableVision");
		enableSelfRighting = Settings.getBoolean("selfRightingEnable");

		ds = DriverStation.getInstance().getEnhancedIO();
		dslcd = DriverStationLCD.getInstance();

		if (visionEnable) {
			cam1 = AxisCamera.getInstance();
		}
		am = AnalogModule.getInstance(1);
		dm = DigitalModule.getInstance(4);

		//initialize driver input options
		leftStick = new Joystick(1);
		rightStick = new Joystick(2);
		joyPad = new Joystick(3);

		//initialize PWM-based outputs
		leftJag = new Jaguar(4, 1);
		rightJag = new Jaguar(4, 2);
		rollerJag = new Victor(4, 3);
		camServo = new Servo(4, 4);

		//initialize the encoders
		leftCoder = new Encoder(1, 2);
		rightCoder = new Encoder(3, 4, true);

		//initalize analog inputs
		gyro = new Gyro(1);


		kicker = new Solenoid[2][2];
		selfRight = new Solenoid[2][2];
		//initialize all the solenoids we'll need (2 per solenoid)
		for (int i = 0; i < 2; i++) {
			for (int j = i; j < i + 2; j++) {
				kicker[i][j - i] = new Solenoid(8, 1 + i + j);
			}
		}
		//dslcdPrintln(1, 1, "kicker init completed");
		//dslcd.updateLCD();
		//Timer.delay(5);
		//try {
		if (enableSelfRighting) {
			selfRight[0][0] = new Solenoid(8, 5);
			selfRight[0][1] = new Solenoid(8, 6);
			selfRight[1][0] = new Solenoid(8, 7);
			selfRight[1][1] = new Solenoid(8, 8);
			//for (int i = 0; i < 4; i++) {
			//	for (int j = i; j < 2 + i; j++) {
			//		if (1 + i + j <= 4) {
			//			selfRight[i][j - i] = new Solenoid(8, 5 + i + j);
			//		}
			//		else {
			//			//selfRight[i][j - i] = new Solenoid(7, 1 + i + j - 4);
			//		}
			//	}
			//}
			setSolenoid(selfRight[0], false);
			setSolenoid(selfRight[1], false);
		}
		pressureSwitch = new DigitalInput(pressurePin);
		compressor = new Relay(compressorPin, Relay.Direction.kForward);

		ultrasonics = new CustomUltrasonics(6, 7, 8, 9);

		encDiff = new EncoderDifference(rightCoder, leftCoder);
		lineDrive = new PIDController(Settings.getDouble("P_drive"),
			   Settings.getDouble("I_drive"),
			   Settings.getDouble("D_drive"), encDiff, leftJag);
		leftCoder.setDistancePerPulse(1 / Settings.getDouble("encoder_ticksPerInch"));
		rightCoder.setDistancePerPulse(1 / Settings.getDouble("encoder_ticksPerInch"));
		lineDrive.setOutputRange(-1, 1);
		lineDrive.setContinuous();
		lineDrive.disable();

		//servoPID = new PIDController();
		//turnPID = new PIDController();
		turnPID = new PIDTurn(Settings.getDouble("P_turn"),
			   Settings.getDouble("I_turn"),
			   Settings.getDouble("D_turn"), .005,
			   leftJag, rightJag, leftCoder, rightCoder, gyro,
			   21.5,
			   Settings.getDouble("encoder_ticksPerInch"),
			   Settings.getDouble("encoder_ticksPerInch"));

		turnPID.setOutputRange(-.65, .65);
		turnPID.setInputRange(-360.0, 360.0);
		turnPID.setTolerance(1 / 90. * 100);
		turnPID.disable();

		position = new PositionAccumulator(leftCoder, rightCoder, gyro, 21.5);

		//setSolenoid(selfRight[0], false);
		//setSolenoid(selfRight[1], false);

		//dslcdPrintln(2, 1, "pneumatics init successful");
		//dslcd.updateLCD();


		//initialize all the variables
		//configuration of things like PID, camera(resolution, brightness)

		toggleKicker(kicker);
		//toggleKicker(kicker);
		/*
		}
		catch (Exception e) {
		String exToS = e.toString();
		String mess = e.getMessage();

		dslcdPrintln(2, 1, "EXCEPTION:");
		dslcdPrintln(3, 1, mess);
		if(mess.length()>21)dslcdPrintln(4, 1, mess.substring(21));
		if(mess.length()>42)dslcdPrintln(5, 1, mess.substring(42));
		dslcdPrintln(6, 1, exToS);
		dslcd.updateLCD();
		}
		 */
	}

	public void loadSwitchValues() {
		int[] ballSwitches = {6, 8, 3, 10, 12, 5, 14, 16, 7};
		int[] robotSwitches = {2, 4, 1};
		try {
			for (int i = 0; i < ballSwitches.length; i++) {
				bPositionSwitch[i] = !ds.getDigital(ballSwitches[i]);
				System.out.print(bPositionSwitch[i] + "  ");
				if (i % 3 == 2) {
					System.out.println();
				}
			}
			System.out.println();
			for (int j = 0; j < robotSwitches.length; j++) {
				rPositionSwitch[j] = !ds.getDigital(robotSwitches[j]);
				System.out.print(rPositionSwitch[j] + "  ");
			}
			System.out.println();
		}
		catch (Exception e) {
			System.out.println("---------------DRIVER STATION I/O EXCEPTION" + e.getMessage() + "\n" + e.toString());
		}

	}

	public void autonomousInit() {
		//enabling/disabling of stuff for autonomous (eg PIDs)
		tempAngle = -1;
		tempDistance = -1;
		autonTurnAngle = 90;
		autonDriveDistance = Settings.getDouble("auto_distance");		//inches
		autonDriveSpeed = -0.25;
		startTime = -1;
		msPeriod = 1000;
		autonomousState = STATE_INIT;

		loadSwitchValues();

		// read driver's station switches and calculate robot position and ball position 
		//-------- calculate robotX ----------------------------------
		if (rPositionSwitch[0] && !rPositionSwitch[1] && !rPositionSwitch[2]) {
			robotX = startX[0];
		}
		else if (!rPositionSwitch[0] && rPositionSwitch[1] && !rPositionSwitch[2]) {
			robotX = startX[1];
		}
		else if (!rPositionSwitch[0] && !rPositionSwitch[1] && rPositionSwitch[2]) {
			robotX = startX[2];
		}
		else {
			robotX = startX[0];  // default toward center of the field
		}

		//--------------- calculate number of ball switches selected ---------------------- 
		numBalls = 0;
		for (switchNum = 0; switchNum < 9; switchNum++) {
//			numBalls += ~bPositionSwitch[switchNum] ; //does this work in Java like it does in C?
			if (bPositionSwitch[switchNum]) {
				numBalls++;
			}
		}

		//---------- calculate robot Y position ---------------------------- 
		if (numBalls == 1) {
			robotY = -173.0;
		}
		else if (numBalls == 2) {
			robotY = -389.0;
		}
		else if (numBalls == 3) {
			robotY = -608.0;
		}
		else {
			robotY = -173.0; // default is offense 
		}

		//---------------------- calculate order and location of balls ----------------------- 
		// we'll make it one ball per row - that'll be a little more straightforward.  
		ballIndex = -1;
		for (switchNum = 0; switchNum < 9; switchNum++) {
			if (bPositionSwitch[switchNum]) {
				ballIndex++;
				ballX[ballIndex] = startX[0];
				ballY[ballIndex] = -108.0 - 216.0 * (numBalls - 1);
				if (switchNum == 0) {
					ballX[ballIndex] = startX[0];
					ballY[ballIndex] = 1 - 44.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 1) {
					ballX[ballIndex] = startX[1];
					ballY[ballIndex] = -144.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 2) {
					ballX[ballIndex] = startX[2];
					ballY[ballIndex] = -144.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 3) {
					ballX[ballIndex] = startX[0];
					ballY[ballIndex] = -108.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 4) {
					ballX[ballIndex] = startX[1];
					ballY[ballIndex] = -108.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 5) {
					ballX[ballIndex] = startX[2];
					ballY[ballIndex] = -108.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 6) {
					ballX[ballIndex] = startX[0];
					ballY[ballIndex] = -72.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 7) {
					ballX[ballIndex] = startX[1];
					ballY[ballIndex] = -72.0 - 216.0 * (numBalls - 1);
				}
				else if (switchNum == 8) {
					ballX[ballIndex] = startX[2];
					ballY[ballIndex] = -72.0 - 216.0 * (numBalls - 1);
				}
				else {
				}
			} // if !bPosSwi...
		} // for switchNum 

		//---- calculate target for each ball (goal, straight to bump, tunnel or tower) ----------- 

		if (numBalls == 1) {
			tgtX[0] = 0.0;
			tgtY[0] = 0.0;
			//if special show-off mode, test for it here 
		}
		else if (numBalls == 2) {
			// tgtX,Y for the tunnel is 138, -232 
			tgtX[0] = 138.0;
			tgtY[0] = -232.0;
			tgtX[1] = 138.0;
			tgtY[1] = -232.0;
		}
		else if (numBalls == 3) {
			// tgtX,Y for the tunnel is 138, -448 
			tgtX[0] = 138.0;
			tgtY[0] = -448.0;
			tgtX[1] = 138.0;
			tgtY[1] = -448.0;
			tgtX[2] = 138.0;
			tgtY[2] = -448.0;
		}
		System.out.println("Robot starting at:   x=" + robotX + "  y=" + robotY);
		//System.out.print("Ball(s) located at:   ");
		System.out.println("number of balls: " + numBalls);
		position.setStartPosition(robotX, robotY);
	} // end autonomousInit 

	/**
	 * This function is called periodically during autonomous
	 */
	public void autonomousPeriodic() {
		//if(isAutonomous())
		generalPeriodic();
		autonomousStateMachine();

		//ultrasonicControl(periodicCounter);
		//if (periodicCounter % 4 == 0) {
		//	dslcdPrintln(3, 1, "EL: " + leftCoder.get() + ",  ER: " + rightCoder.get() + "   ");
		//	dslcd.updateLCD();
		//}
		//periodicCounter++;
	}

	public void autonomousContinuous() {
		//will need "if(isEnabled())" and if(isDisabled())
//		if (isEnabled()) {
//		}
	}
	double tempAngle;
	double tempDistance;
	double autonTurnAngle;
	double autonDriveDistance;		//inches
	double autonDriveSpeed;
	long startTime;
	long msPeriod;
	boolean needZeroPivot = false;
	boolean firstTimeThru = true;

	public double calculateDriveDistance(int ballIndex, double[] pos) {
		// if one or two balls, return 65.0 // inches
		// if three, return 70.0 // inches 
		return Math.sqrt(MathUtils.pow((ballX[ballIndex] - pos[0]), 2)
			   + MathUtils.pow((ballY[ballIndex] - pos[1]), 2));
	}

	public double calculateDriveDistance(int nBalls) {
		if (nBalls == 1 || nBalls == 2) {
			return 31.0;
		}
		else if (nBalls == 3) {
			return 38.0;
		}
		else {
			return 0;
		}
	}

	public double calculateTurnAngle(int targetIndex, double[] pos) {
		return MathUtils.atan2(tgtX[targetIndex] - pos[0], tgtY[targetIndex] - pos[1]) * 180 / Math.PI;
	}

	public double calculateTurnAngle(int nBalls) {
		if (nBalls == 1) {
			return MathUtils.atan2(-92.0, 144.0) * 180.0 / Math.PI;
		}
		else //if (nBalls > 1) {
		{
			return MathUtils.atan2(45.0, 128.0) * 180.0 / Math.PI;
		}
		//}
	}

	public boolean autonomousDriveDistance(double speed, double distance) {
		if (tempDistance == -1) {
			tempDistance = rightCoder.getDistance();
			autonDriveDistance += rightCoder.getDistance();
			rightJag.set(speed);
			leftJag.set(speed);
			lineDrive.enable();
		}
		if (Math.abs(rightCoder.getDistance() - distance) < 1.0) {
			lineDrive.disable();
			lineDrive.reset();
			rightJag.set(0);
			leftJag.set(0);
			return true;
		}
		return false;
	}

	public boolean autonomousTurnAngle(double degrees) {
		if (turnPID.getSetpoint() == 0) {
			turnPID.enable();
			turnPID.setSetpoint(gyro.getAngle() + degrees);
		}
		//if (Math.abs(turnPID.getError()) < 2) {
		//if(Math.abs(gyro.getAngle()-tempAngle)  tempAngle)
		if (turnPID.onTarget() && //Math.abs(turnPID.get()) < .1 &&
			   Math.abs(gyro.getAngle() - turnPID.getSetpoint()) < 1) {
			turnPID.disable();
			return true;
		}
		return false;
	}

	public boolean autonomousKick(double time) {
		//case STATE_KICK:
		rollerJag.set(0);
		//kick
		if (startTime == -1) {
			toggleKicker(kicker);
			startTime = Timer.getUsClock();
		}
		if ((Timer.getUsClock() - startTime) / 1000 > time) {
			startTime = -1;
			toggleKicker(kicker);
			//autonomousState = -1;
			return true;
		}
		return false;
	}

	public void autonomousStateMachine() {
		double[] pos = position.getPosition();
		switch (autonomousState) {//init
			case STATE_INIT:
				if (startTime == -1) {
					startTime = Timer.getUsClock();
					leftCoder.reset();
					leftCoder.start();
					rightCoder.reset();
					rightCoder.start();
					lineDrive.disable();
					lineDrive.reset();
					turnPID.disable();
					turnPID.reset();
					//if ( Math.abs(ballX[1]-robotX) > 6.0 ) needZeroPivot = true ;
					dslcdPrintln(2, 1, "entering state " + autonomousState + "...");
					dslcd.updateLCD();
				}
				if ((Timer.getUsClock() - startTime) / 1000 > msPeriod) {
					rollerJag.set(rollDirection);
					startTime = -1;
					autonomousState = STATE_DRIVE_FIRST_BALL;
					//autonomousState = STATE_PIVOT_FIRST_BALL;
					System.out.println("init completed.  driving to ball...");

				}
				//timer delay
				//	timer expires: next step
				//roller on
				break;
			// if ballX[1] isn't the same as the robot's, we have to pivot first

			case STATE_PIVOT_FIRST_BALL:
				/*
				if (needZeroPivot) {
				if (firstTimeThru) { // calculate angle to first ball
				// note atan2 coefficients are unusual since zero angle is along the Y axis
				// and angle increases clockwise (rather than X axis, CCW)
				angle = MathUtils.atan2(ballX[0] - robotX, ballY[0] - robotY);
				firstTimeThru = false;
				}
				//run pivot until it's done
				//when done set autonomousState = STATE_DRIVE_FIRST_BALL ;
				}
				else {
				// - advance to next state
				 */
				autonomousState = STATE_DRIVE_FIRST_BALL;
				/*
				firstTimeThru = true;
				}

				 */
				break;

			case STATE_DRIVE_FIRST_BALL:
				//case STATE_DRIVE:
				//drive @ distance1, speed1
				//distance error to zero:next step
				// need to calculate distance from robot position to ball position
				if (firstTimeThru) { // calculate distance to ball

					System.out.println(ballX[0] + "  " + pos[0] + "  " + ballY[0] + "  " + pos[1]);
					//autonDriveDistance = calculateDriveDistance(0, pos);
					autonDriveDistance = calculateDriveDistance(numBalls);
					System.out.println("Driving " + autonDriveDistance + " inches...");
					dslcdPrintln(2, 1, "entering state " + autonomousState + "...");
					dslcd.updateLCD();
					firstTimeThru = false;
				}
				if (autonomousDriveDistance(autonDriveSpeed, autonDriveDistance)) {
					//autonomousState = STATE_TURN;
					autonomousState = STATE_PIVOT_FIRST_TARGET;
					System.out.println("finished driving, commencing turn");
					firstTimeThru = true;
				}
				break;

			case STATE_PIVOT_FIRST_TARGET:
				//case STATE_TURN:
				//pivot @ angle (degrees)	-----------------NEED PID FOR TURN CONTROLLING
				//	angle error to zero:next step
				//turnPID.setSetpoint(degrees);
				//if (firstTimeThru) { // calculate pivot angle
				if (firstTimeThru) { // calculate angle to goal
					// note atan2 coefficients are unusual since zero angle is along the Y axis
					//and angle increases clockwise
					//(rather than X axis, CCW)

					//autonTurnAngle = calculateTurnAngle(0, pos);
					autonTurnAngle = calculateTurnAngle(numBalls);
					System.out.println("Turning " + autonTurnAngle + " degrees");
					firstTimeThru = false;
					dslcdPrintln(2, 1, "entering state " + autonomousState + "...");
					dslcd.updateLCD();
				}
				//}
				if (autonomousTurnAngle(autonTurnAngle)) {
					//autonomousState = STATE_KICK;
					autonomousState = STATE_FIRST_KICK;
					System.out.println("turned.  firing...");
				}
				break;

			case STATE_FIRST_KICK:
				if (autonomousKick(1000)) {
					//if (numBalls > 1) {
						autonomousState = STATE_POST_FIRST_KICK;
						dslcdPrintln(2, 1, "entering state " + autonomousState + "...");
						dslcd.updateLCD();
					//}
					// get rid of this next section, just go for one ball 
					//else {
					//	autonomousState = STATE_PIVOT_LINE;
					//}
					//System.out.println("ball fired.  exiting state machine.");
				}
				break;

			//wait small amount of time after kick
			case STATE_POST_FIRST_KICK:
				//	timer to zero: retract kicker, next step
				// NO - just skip the rest!
				autonomousState = STATE_DONE ;
				//autonomousState = STATE_PIVOT_SECOND_BALL;
				break;

			case STATE_PIVOT_SECOND_BALL:
				autonomousState = STATE_DRIVE_SECOND_BALL;
				break;

			case STATE_DRIVE_SECOND_BALL:
				autonomousState = STATE_PIVOT_SECOND_TARGET;
				break;

			case STATE_PIVOT_SECOND_TARGET:
				autonomousState = STATE_SECOND_KICK;
				break;

			case STATE_SECOND_KICK:
				if (autonomousKick(1000)) {
					if (numBalls > 2) {
						autonomousState = STATE_POST_SECOND_KICK;
					}
					else {
						autonomousState = STATE_PIVOT_LINE;
					}
					//System.out.println("ball fired.  exiting state machine.");
				}
				break;

			case STATE_POST_SECOND_KICK:
				autonomousState = STATE_PIVOT_THIRD_BALL;
				break;

			case STATE_PIVOT_THIRD_BALL:
				autonomousState = STATE_DRIVE_THIRD_BALL;
				break;

			case STATE_DRIVE_THIRD_BALL:
				autonomousState = STATE_PIVOT_THIRD_TARGET;
				break;

			case STATE_PIVOT_THIRD_TARGET:
				autonomousState = STATE_THIRD_KICK;
				break;

			case STATE_THIRD_KICK:
				autonomousState = STATE_POST_THIRD_KICK;
				break;

			case STATE_POST_THIRD_KICK:
				autonomousState = STATE_PIVOT_LINE;
				break;

			case STATE_PIVOT_LINE:
				autonomousState = STATE_DRIVE_LINE;
				break;

			case STATE_DRIVE_LINE:
				autonomousState = STATE_JUST_LOOK_MENACING;
				break;

			case STATE_JUST_LOOK_MENACING:
				autonomousState = STATE_DONE;
				break;

			case STATE_DONE:
				if (firstTimeThru) {
					dslcdPrintln(2, 1, "AUTONOMOUS FINISHED");
					dslcd.updateLCD();
				}
				break;
		}

		//if(!last step){
		//	autonomousState = STATE_TURN


		//pivot @ angle (degrees)
		//	angle error to zero: next step

		//drive out of way @ speed2, distance2
		//	distance error to : next step

		//wait for autonomous to end

	}

	public void disabledInit() {
		if (servoPID != null) {
			servoPID.disable();
			servoPID.reset();
		}

		if (turnPID != null) {
			turnPID.disable();
			turnPID.reset();
		}

		if (lineDrive != null) {
			lineDrive.disable();
			lineDrive.reset();
		}

	}
	/*
	public void disabledPeriodic() {
	if (ds.getDigitalIn(8)) {
	Settings.reload();
	lineDrive.setPID(Settings.getDouble("P_drive"), Settings.getDouble("I_drive"), Settings.getDouble("D_drive"));
	if(servoPID!=null){
	servoPID.setPID(Settings.getDouble("P_servo"), Settings.getDouble("I_servo"), Settings.getDouble("D_servo"));
	}
	}

	}
	 */

	public void disabledContinuous() {
	}

	public void disabledPeriodic() {
		generalPeriodic();
	}

	public void teleopInit() {
		leftCoder.start();
		rightCoder.start();
		leftCoder.reset();
		rightCoder.reset();
	}

	/**
	 * This function is called periodically during operator control
	 */
	public void teleopPeriodic() {
		generalPeriodic();


		//ultrasonicControl(periodicCounter);
		//includes
		//sending back DS data
	}

	public void teleopContinuous() {
		if (isEnabled()) {
			if (leftStick.getRawButton(1) && rightStick.getRawButton(1)) {
				boolean rtPOINT = rightStick.getRawButton(jsPOINT),
					   rtSTRAIGHT = rightStick.getRawButton(jsSTRAIGHT),
					   rtTRACK = rightStick.getRawButton(jsTRACK),
					   lePOINT = leftStick.getRawButton(jsPOINT),
					   leSTRAIGHT = leftStick.getRawButton(jsSTRAIGHT),
					   leTRACK = leftStick.getRawButton(jsTRACK);



				double leftVal = leftStick.getY();
				double rightVal = rightStick.getY();

				if (leTRACK || rtTRACK) {
				}
				else if (lePOINT || rtPOINT) {
					if (lePOINT) {
						rightVal = leftVal;
					}
					else if (rtPOINT) {
						leftVal = rightVal;
					}

				}
				else if (!isDrivingStraight && (leSTRAIGHT || rtSTRAIGHT)) {
					lineDrive.reset();
					leftCoder.reset();
					rightCoder.reset();
					rightJag.set(rightVal);
					leftJag.set(-rightVal);
					lineDrive.enable();
					isDrivingStraight =
						   true;
				}
				else if (isDrivingStraight && (leSTRAIGHT || rtSTRAIGHT)) {
					rightJag.set(rightVal);
				}
				else if (isDrivingStraight && (!leSTRAIGHT && !rtSTRAIGHT)) {
					lineDrive.disable();
					isDrivingStraight =
						   false;
				}

				if (!leSTRAIGHT && !rtSTRAIGHT) {
					leftJag.set(-leftVal);
					rightJag.set(rightVal);
				}

			}
			else {
				leftJag.set(0);
				rightJag.set(0);
			}

			if (enableSelfRighting //&& leftJag.get() == 0.0 && rightJag.get() == 0.0
				   && (joyPad.getRawButton(9) || joyPad.getRawButton(10))) {// ||
				//joyPad.getRawButton(11) || joyPad.getRawButton(12))) {
				if (joyPad.getRawButton(jpU)) {
					deploySolenoid(selfRight[pnFRONT], selfRightDeployTime);
				}
				else if (joyPad.getRawButton(jpD)) {
					deploySolenoid(selfRight[pnBACK], selfRightDeployTime);
				}
				else if (joyPad.getRawButton(jpL)) {
					//deploySolenoid(selfRight[pnLEFT], selfRightDeployTime);
					setSolenoid(selfRight[0], true);
					Timer.delay(6);
					setSolenoid(selfRight[0], false);
					System.out.println("LEFT SIDE SELF-RIGHTING DEPLOYED");
				}
				else if (joyPad.getRawButton(jpR)) {
					setSolenoid(selfRight[1], true);
					Timer.delay(6);
					setSolenoid(selfRight[1], false);
					System.out.println("RIGHT SIDE SELF-RIGHTING DEPLOYED");
				}
			}
			//kick toggle button has been pressed
			else if (jpKICKhasChangedState && (joyPad.getRawButton(jpKICK) || joyPad.getRawButton(jpKICK + 2) /*|| ds.getDigitalIn(4)*/ || leftStick.getRawButton(7))) {
				//rollerJag.set(-rollDirection);
				Timer.delay(.005);
				toggleKicker(kicker);
				jpKICKhasChangedState =
					   false;
			}
			//button to kick has been released and wasn't last time it checked
			else if (!jpKICKhasChangedState && !joyPad.getRawButton(jpKICK) && !joyPad.getRawButton(jpKICK + 2) /*&& !ds.getDigitalIn(4)*/ && !leftStick.getRawButton(7)) {
				jpKICKhasChangedState = true;
			}
			else if (!kickerIsOut) {
				//rollerJag.set(-rollDirection);
			}
			//roller isn't rolling but it should be
			else if (joyPad.getRawButton(jpROLL) || joyPad.getRawButton(jpROLL + 2) || leftStick.getRawButton(6)) {
				rollerJag.set(rollDirection);
			}
			//roller shouldn't be rolling
			else {
				rollerJag.set(0);
			}

			//if (kickerIsOut) {
			//	dslcdPrintln(2, 1, "kicker OUT    ");
			//}
			//else {
			//	dslcdPrintln(2, 1, "kicker NOT OUT");
			//}
			//if (isDrivingStraight) {
			//	dslcdPrintln(2, 16, "::PLL");
			//}
			//else {
			//	dslcdPrintln(2, 15, "     ");
			//}
			dslcd.updateLCD();
		}
		//if the robot isn't enabled
		else {
			leftJag.set(0);
			rightJag.set(0);
			lineDrive.disable();
		}
		//includes:
		// anything the user needs to control
		//   driving (jaguars), shooting (solenoids)
		//

	}
	boolean shiftLeft = true;

	public void generalPeriodic() {
		position.updatePosition();
		getWatchdog().feed();
		if (!pressureSwitch.get() && !compressorIsOn && pneumaticsEnable) {
			compressor.set(Relay.Value.kOn);
			compressorIsOn =
				   true;
		}
		else if (pressureSwitch.get() && compressorIsOn && pneumaticsEnable) {
			compressor.set(Relay.Value.kOff);
			compressorIsOn =
				   false;
		}

		try {
			//if (/*periodicCounter % 8 == 0 && */!ds.getDigital(reloadSettings - 1) && isDisabled()) {
			//	refreshSettings();
			//}
		}
		catch (Exception e) {
			System.out.println("---------------DRIVER STATION I/O EXCEPTION" + e.getMessage() + "\n" + e.toString());
		}

		dXaccum += position.chgX;
		dYaccum += position.chgY;

		if (periodicCounter % 3 == 0) {
			if (shiftLeft) {
				psocLEDs = (byte) Math.abs(psocLEDs << 1);
			}
			else {
				psocLEDs = (byte) Math.abs(psocLEDs >> 1);
			}
			if (psocLEDs == 128 || psocLEDs == -128) {
				shiftLeft = false;
			}
			else if (psocLEDs == 1) {
				shiftLeft = true;
			}
			try {
				ds.setLEDs(psocLEDs);
				//System.out.println(psocLEDs);
			}
			catch (Exception e) {
				System.out.println("DS Enhanced I/O Exception!!");
			}
		}
		//if (periodicCounter % 8 == 0) {
		//	//dslcdPrintln(3, 1, "EL: " + leftCoder.get() + ",  ER: " + rightCoder.get() + "   ");
		//	double[] pos = position.getPosition();
		//	dslcdPrintln(1, 1, "dX=" + (float) dXaccum + "   ");
		//	dslcdPrintln(2, 1, "dY=" + (float) dYaccum + "   ");
		//	dslcdPrintln(3, 1, "X: " + (float) pos[0] + "   ");
		//	dslcdPrintln(4, 1, "Y: " + (float) pos[1] + "   ");
		//	dslcdPrintln(5, 1, "A: " + (float) pos[2] + "   ");
		//	dslcdPrintln(6, 1, "G: " + (float) gyro.getAngle() + "   ");
		//	dslcd.updateLCD();
		//	dXaccum = 0;
		//	dYaccum = 0;
		//}
		periodicCounter++;
	}
	double dXaccum = 0, dYaccum = 0;

	public void ultrasonicControl(long count) {
		if (enableUltrasonics) {
			if (count == 0) {
				ultrasonics.startPinging();
			}
			else if (count == 1) {
				ultrasonics.stopPinging();
			}
			else if (count == 7) {
				System.out.println(ultrasonics.getDistances());
				periodicCounter = 0;
			}

		}
	}

	public void deploySolenoid(Solenoid[] sols, double period) {
		sols[0].set(true);
		sols[1].set(false);
		Timer.delay(period);
		sols[0].set(false);
		sols[1].set(true);
	}

	public void deploySolenoid(Solenoid[] sols, int watchButton) {
		sols[0].set(true);
		sols[1].set(false);
		//wait until the button is released
		while (joyPad.getRawButton(watchButton)) {
			Timer.delay(.0001);
		}

		sols[0].set(false);
		sols[1].set(true);
	}

	public void setSolenoid(Solenoid[] sols, boolean state) {
		//boolean solstate = sols[0].get();
		sols[0].set(state);
		sols[1].set(!state);
	}

	public void toggleKicker(Solenoid[][] kick) {
		//if (kickerCanChangeState) {
		setSolenoid(kick[0], kickerIsOut);
		setSolenoid(kick[1], kickerIsOut);
		kickerIsOut =
			   !kickerIsOut;
		//kickerCanChangeState = false;
		//}
		//else {
		//	toggleSolenoid(kick[0]);
		//	toggleSolenoid(kick[1]);
		//	kickerCanChangeState = false;
		//}
	}

	public void dslcdPrintln(int line, int startCol, String message) {
		switch (line) {
			case 1:
				dslcd.println(DriverStationLCD.Line.kMain6, startCol, message);
				break;

			case 2:
				dslcd.println(DriverStationLCD.Line.kUser2, startCol, message);
				break;

			case 3:
				dslcd.println(DriverStationLCD.Line.kUser3, startCol, message);
				break;

			case 4:
				dslcd.println(DriverStationLCD.Line.kUser4, startCol, message);
				break;

			case 5:
				dslcd.println(DriverStationLCD.Line.kUser5, startCol, message);
				break;

			case 6:
				dslcd.println(DriverStationLCD.Line.kUser6, startCol, message);
				break;

		}
	}

	public void refreshSettings() {
		Settings.reload();
		enableSelfRighting =
			   Settings.getBoolean("selfRightingEnable");
		enableUltrasonics =
			   Settings.getBoolean("ultrasonicsEnable");
		pneumaticsEnable =
			   Settings.getBoolean("pneumaticsEnable");
		visionEnable =
			   Settings.getBoolean("visionEnabled");
		if (lineDrive != null) {
			lineDrive.disable();
			lineDrive.setPID(Settings.getDouble("P_drive"),
				   Settings.getDouble("I_drive"),
				   Settings.getDouble("D_drive"));
		}

		if (servoPID != null && visionEnable) {
			servoPID.setPID(Settings.getDouble("P_servo"),
				   Settings.getDouble("I_servo"),
				   Settings.getDouble("D_servo"));
		}

		if (turnPID != null) {
		}
		Timer.delay(.5);
	}

	public void turn(int degrees, double speed) {
	}

	public void driveStraight(float distance, double speed) {
	}
}
